#include "accelerometer.h"
#include "debug_print.h"

#define ACC_SAMPLE_COUNT 4
#define ACC_POINT_COUNT ((ACC_SAMPLE_COUNT + 1) * 2)

typedef struct ACC_T
{
  uint16        points[ACC_POINT_COUNT];
  bool          is_up[ACC_POINT_COUNT];
  uint8         point_index;
  ACC_HANDLER_T handler;
} ACC_T;

ACC_T g_acc;
ACC_T* acc;

#define acc_instance() (&g_acc)

void acc_init(void)
{
  acc = acc_instance();
  acc->point_index = 0;
  acc->handler = 0;
  timera_capture_init();

  // power up
  P2DIR |= BIT7;
  P2OUT |= BIT7;
}

void acc_finish(void)
{
    timera_capture_stop();
    P2OUT &= ~BIT7;
}

void acc_capture_handler(bool is_up, uint16 cycle)
{
  // we always need up edige as first point
  if (acc->point_index == 0)
  {
    if (is_up == false)
    {
      return;
    }
  }
  acc->points[acc->point_index] = cycle;
  acc->is_up[acc->point_index] = is_up;
  if (++acc->point_index == ACC_POINT_COUNT)
  {
//    acc->point_index = 0;
      // we are done
    timera_capture_stop();
    // we have collected all points
    uint8 i;
    uint8 count = 0;
    uint16 valid_count = 0;
    uint16 d = 0;
    uint32 total_t1 = 0, total_t2 = 0;
    uint16 t1_array[ACC_SAMPLE_COUNT];
    uint16 t2_array[ACC_SAMPLE_COUNT];
    uint16 min_t1=65535, max_t1=0;
    uint8 min_index = 0, max_index = 0;

    INTERRUPT_DISABLE();
    // ignore the first up point, because it does not contain full cycle
    for (i = 1; i < ACC_POINT_COUNT; i++)
    {
      uint16 p = acc->points[i];
      if (acc->is_up[i] == false)//?????????
        d = p;
      else
      {
        t1_array[count] = d;
        t2_array[count] = d + p;
        if (d < min_t1)
        {
          min_index = count;
          min_t1 = d;
        }
        if (d > max_t1)
        {
          max_index = count;
          max_t1 = d;
        }
        count++;
      }
    }
    INTERRUPT_ENABLE();

    valid_count = 0;
    total_t1 = 0;
    total_t2 = 0;
    for (i = 0; i < count; i++)
    {
      if (i != min_index && i != max_index)
      {
        total_t1 += t1_array[i];
        total_t2 += t2_array[i];
        valid_count++;
      }
    }
    total_t1 /= valid_count;
    total_t2 /= valid_count;
    if (acc->handler != 0)
    {
      acc->handler((uint16)total_t1, (uint16)total_t2);
    }
    acc->point_index = 0;
  }
}

void acc_start_capture(ACC_HANDLER_T handler)
{
  acc = acc_instance();
  acc->handler = handler;
  INTERRUPT_DISABLE();
  timera_capture_stop();
  acc->point_index = 0;
  INTERRUPT_ENABLE();

  timera_capture_start(acc_capture_handler);
}
